// Camera.cpp
#include "Camera.h"
#include "Car.h"
#include <QGraphicsView>
#include <QPainter>
#include <QtMath>

Camera::Camera(QGraphicsScene* scene, Car* car, QObject* parent)
    : QObject(parent), m_scene(scene), m_car(car) {}

QImage Camera::captureView() const {
    // 获取小车的位置和朝向
    QPointF carPos = m_car->pos();  // 小车的位置
    qreal carAngle = m_car->rotation();  // 小车的朝向角度，单位为度

    // 将角度转换为弧度
    qreal angleRad = qDegreesToRadians(carAngle);

    // 计算视角矩形的中心点，矩形应当位于小车正前方
    qreal offsetX = m_viewWidth+130;
    qreal offsetY = 0;

    // 手动旋转坐标
    qreal rotatedX = carPos.x() + offsetX * qCos(angleRad);
    qreal rotatedY = carPos.y() + offsetX * qSin(angleRad);

    // 视角矩形的中心点
    QPointF viewCenter(rotatedX, rotatedY);

    // 创建一个临时视图来渲染图像
    QGraphicsView view(m_scene);

    // 设置视图的场景矩形
    view.setSceneRect(viewCenter.x() - m_viewWidth / 2, viewCenter.y() - m_viewHeight / 2,
                      m_viewWidth, m_viewHeight);
    // 创建一个旋转变换来旋转视图
    QTransform transform;
    transform.rotate(-carAngle);  // 将视图旋转为小车的朝向
    //在image中小车位置始终为(0,height/2)

    // 应用旋转变换
    view.setTransform(transform);
    // 创建 QImage 用于渲染视图
    QImage image(m_viewWidth, m_viewHeight, QImage::Format_RGB32);
    QPainter painter(&image);
    view.render(&painter);  // 将视图渲染到 QImage 上

    return image;
}


QPoint Camera::processImage(const QImage& image) const {
    int sumY = 0, sumX = 0,count = 0;   

    int target_x, target_y; 
    double distance, d_theta;
    double distance_total = std::numeric_limits<double>::max();

    //基于image小车自身坐标系找到目标点
    //遍历image中黑色轨迹就是轨迹点，在黑色的轨迹上选目标点
    for (int y = 0; y < image.height(); ++y) {
        const uchar* scanLine = image.scanLine(y);
        for (int x = 0; x < image.width(); ++x) {
            QRgb pixel = reinterpret_cast<const QRgb*>(scanLine)[x];
            if (qGray(pixel) < 50) {  // 检测黑色轨迹
                // if(x>=0 && x <5)
                // {
                //     sumY += y;
                //     sumX += x;
                //     ++count;
                // }
                double dx = x - 0;
                double dy = y - image.height()/2;
                distance = std::sqrt(dx * dx + dy * dy);
                d_theta = std::atan2(dy, dx);
                if(5*fabs(d_theta)+ distance < distance_total){
                        distance_total = 5*fabs(d_theta)+distance;
                        target_x = x;
                        target_y = y;
                }
                ++count;
            }
        }
    }
    std::cout<<"target_x = "<<target_x<<"target_y = "<<target_y<<std::endl;
    //轨迹跟随法 计算车道线中心位置与相机范围中心相比，是在上方w<0,还是下方w>0
    //QPoint result = (count > 0) ? QPoint(sumX / count, sumY / count - image.height()/2) : QPoint(-1, -1);
    QPoint result = (count > 0) ? QPoint(target_x, target_y - image.height()/2) : QPoint(-1, -1);
    std::cout << "return = (" << result.x() << ", " << result.y() << ")" << std::endl;
    return result;
}
